import math
import helpers
import heapq


def shortest_path(M, start, goal):
    def get_h(a, b):
        return math.sqrt(
            (M.intersections[a][0] - M.intersections[b][0]) ** 2 + (M.intersections[a][1] - M.intersections[b][1]) ** 2)

    frontier = set()
    priority_queue = []
    explored = set()
    node = {}
    node[start] = {'g': 0,
                   'h': get_h(start, goal),
                   'f': 0 + get_h(start, goal),
                   'parent': None}

    frontier.add(start)
    heapq.heappush(priority_queue, (node[start]['f'], start))
    while frontier:
        current_node = heapq.heappop(priority_queue)[1]
        for adjacent in M.roads[current_node]:
            if adjacent not in explored:
                tmp = (node[current_node]['g'] + get_h(current_node, adjacent) + get_h(adjacent, goal))
                if (adjacent not in frontier) or (tmp < node[adjacent]['f']):
                    new_g = node[current_node]['g'] + get_h(current_node, adjacent)
                    node[adjacent] = {'g': new_g,
                                      'h': get_h(adjacent, goal),
                                      'f': new_g + get_h(adjacent, goal),
                                      'parent': current_node}
                    frontier.add(adjacent)
                    heapq.heappush(priority_queue, (node[adjacent]['f'], adjacent))
        if goal in explored:
            return generate_path(goal, node)
        if current_node in frontier:
            frontier.remove(current_node)
            explored.add(current_node)


def generate_path(goal, node):
    path = []
    tmp = goal
    while tmp:
        path.append(tmp)
        tmp = node[tmp]['parent']
    path.reverse()
    return path
